/**
  ****************************(C) COPYRIGHT 2023 Polarbear*************************
  * @file       usb_task.c/h
  * @brief      usb outputs the IMU and gimbal data to the miniPC
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023-7-11       Penguin         1. done
  *  V1.0.1     Oct-31-2023     LihanChen       1. 完成核心框架的构建，使其与调试模式和 miniPC 模式兼容。
  *  V1.0.2     Nov-1-2023      LihanChen       1. 将 Append_CRC16_Check_Sum_SendPacketVision() 和 Append_CRC-16_Check_Stum_OutputPCData() 合并到 Append_CRC16_Check_Sum() 中
  *  v2.0.0     Feb-24-2024     LihanChen       1. 重构代码，将 usb_task 的发送和接收分离，并实现分包发送和接收视觉和导航数据
  *  v2.0.1     Dec-25-2024     HaoyangZhang    1. 重构代码，删除不必要变量，去除自瞄部分移交算法
  *
  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2023 Polarbear*************************
*/


#ifndef __MINIPC_HPP__
#define __MINIPC_HPP__

#include "main.h"
#include "usbd_cdc_if.h"
#include "math.h"
#include "bsp_imu.hpp"
#include "gimbal.hpp"
//#include "shoot.hpp"
#include "chassis.hpp"

#define PI 3.1415926535f
#define GRAVITY 9.78f

#define MINIPC_ONLINE	1
#define MINIPC_LOST		0

#define OUTPUT_VISION_STATE 0
#define RECEIVE_VISION_STATE 2
#define RECEIVE_NAVIGATION_STATE 3

#define EXPECTED_INPUT_NAVIGATION_HEDER 0x4A
#define SET_OUTPUT_NAVIGATION_HEDER 0xA6
#define EXPECTED_INPUT_VISION_HEDER 0xA5
#define SET_OUTPUT_VISION_HEDER 0x5A

#define CRC16_INIT 0xFFFF
#define CRC8_INIT 0xFF
#define USB_STATE_INVALID 0xFF
#define APP_RX_DATA_SIZE 2048
#define APP_TX_DATA_SIZE 2048
#define USB_RECEIVE_LEN 384

typedef struct {
    fp32 shoot_angular_;
    fp32 yaw_angular_;
} minipc_recv_behavior_t;

typedef struct {
	uint8_t shoot_state_;
    float shoot_angular_;
		float yaw_angular_;
} minipc_recv_vison_behavior_t;

typedef struct {
	fp32 Twist_x;
    fp32 Twist_angular;
	uint8_t spinning_flag;
} minipc_recv_Twist_behavior_t;

typedef struct{
	float vison[3];
//	float vison_yaw;
//	float vison_shoot_state;
  uint8_t fire;

	float ros_x;
	float ros_angular;
	uint8_t ros_spinning_flag;
} USBReceiveData;

typedef struct
{
  uint8_t header ;
  uint8_t state : 2;       // 0-untracking 1-tracking-aim 2-tracking-buff
  uint8_t id : 3;          // aim: 0-outpost 6-guard 7-base
  uint8_t armors_num : 3;  // 2-balance 3-outpost 4-normal
	uint8_t fire;
  float x;                 // aim: robot-center || buff: rune-center
  float y;                 // aim: robot-center || buff: rune-center
  float z;                 // aim: robot-center || buff: rune-center
  float yaw;               // aim: robot-yaw || buff: rune-theta
  float vx;  // aim: robot-vx || buff: rune spin speed param - a
  float vy;  // aim: robot-vy || buff: rune spin speed param - b
  float vz;  // aim: robot-vz || buff: rune spin speed param - w
  float v_yaw;
  float r1;
  float r2;
  float dz;
  uint32_t cap_timestamp;  // (ms) frame capture time
  uint16_t t_offset;       // (ms) speed t offset
  float change_pitch;
  float change_yaw;
  uint16_t checksum ;
}__attribute__((packed)) ReceivedPacketVision_s;

typedef struct
{
  uint8_t header;
  uint8_t detect_color : 1;  // 0-red 1-blue
  uint8_t task_mode : 2;     // 0-auto 1-aim 2-buff
  bool reset_tracker : 1;
  uint8_t is_play : 1;
  bool change_target : 1;
  uint8_t reserved : 2;
  float roll;
  float pitch;
  float yaw;
  float aim_x;
  float aim_y;
  float aim_z;
  uint16_t game_time;  // (s) game time [0, 450]
  uint32_t timestamp;  // (ms) board time
  uint16_t checksum;
} __attribute__((packed)) SendPacketVision_s;

typedef struct
{
	uint8_t header;
	uint16_t behavior_chassis_vx;
	uint16_t behavior_chassis_vy;
	uint16_t checksum;
} __attribute__((packed)) SendPacketTwist_s;

typedef struct
{
	uint8_t header;
	float speed_x;
	float angular;
	uint8_t spinning_flag;
	uint16_t checksum;
} __attribute__((packed)) ReceivedPacketTwist_s;

static SendPacketVision_s SendPacketVision;
static SendPacketTwist_s SendPacketTwist;
static ReceivedPacketVision_s ReceivedPacketVision;
static ReceivedPacketTwist_s ReceivedPacketTwist;
static USBReceiveData usb_receive_data;
//extern USBReceiveData usb_receive_data;
//static minipc_recv_vison_behavior_t minipc_vison_behavior_t;
//static minipc_recv_Twist_behavior_t minipc_twist_behavior_t;

//extern minipc_recv_vison_behavior_t minipc_vison_behavior_t;
//extern minipc_recv_Twist_behavior_t minipc_twist_behavior_t;

// Function Prototypes
void Minipc_init(void);
static void usb_send_vision(void);
static void usb_send_Twist(void);
static void usb_receive(void);
static void usb_receive_navigation(ReceivedPacketTwist_s *ReceivedPacketTwist, USBReceiveData *usb_receive_data);
static void usb_receive_vision(ReceivedPacketVision_s *ReceivedPacketVision, USBReceiveData *usb_receive_data);
static void minipc_thread_entry(void *parameter);

uint16_t Get_CRC16_Check_Sum(const uint8_t *pchMessage, uint32_t dwLength, uint16_t wCRC);
uint32_t Verify_CRC16_Check_Sum(const uint8_t *pchMessage, uint32_t dwLength);
void Append_CRC16_Check_Sum(void *pchMessage, uint32_t dwLength, int dataType);

extern uint8_t MiniPC_Receive_Vision(float *pitch, float *yaw);
extern uint8_t MiniPC_Receive_Twist(float *ros_x_speed, float *ros_y_speed, uint8_t *spinning_flag);

#endif
